#include <windows.h>
#include "quat.h"
#pragma hdrstop

QUAT::QUAT(GLfloat sx, GLfloat sy, GLfloat sz, GLfloat sw)
:
	x(sx),
	y(sy),
	z(sz),
        w(sw)
{
}

QUAT::~QUAT()
{
}

GLvoid QUAT::Reset()
{
	x = 0.0;
	y = 0.0;
	z = 0.0;
	w = 1.0;
}

GLvoid QUAT::CopyQuat(QUAT q)
{
	x = q.x;
	y = q.y;
	z = q.z;
	w = q.w;
}

GLvoid QUAT::AxisAngleToQuat(VECTOR axis, GLfloat theta)
{
        GLfloat halfTheta = theta * 0.5;
        GLfloat cosHalfTheta = cos(halfTheta);
        GLfloat sinHalfTheta = sin(halfTheta);
        x = axis.x * sinHalfTheta;
        y = axis.y * sinHalfTheta;
        z = axis.z * sinHalfTheta;
        w = cosHalfTheta;
}

GLvoid QUAT::EulerToQuat(GLfloat roll, GLfloat pitch, GLfloat yaw)
{
    	GLfloat cr, cp, cy, sr, sp, sy, cpcy, spsy;  // calculate trig identities
    	cr = cos(roll/2);
    	cp = cos(pitch/2);
    	cy = cos(yaw/2);
    	sr = sin(roll/2);
    	sp = sin(pitch/2);
    	sy = sin(yaw/2);
    	cpcy = cp * cy;
    	spsy = sp * sy;
    	w = cr * cpcy + sr * spsy;
    	x = sr * cpcy - cr * spsy;
    	y = cr * sp * cy + sr * cp * sy;
    	z = cr * cp * sy - sr * sp * cy;
}

GLfloat QUAT::MagnitudeQuat()
{
  	return( sqrt(w*w+x*x+y*y+z*z));
}

GLvoid QUAT::NormaliseQuat()
{
  	GLfloat Mag;
  	Mag = MagnitudeQuat();
  	w = w/Mag;
  	x = x/Mag;
  	y = y/Mag;
  	z = z/Mag;
}

GLvoid QUAT::MultQuat(QUAT q)
{
  	QUAT q3;
  	VECTOR vectorq1;
  	VECTOR vectorq2;
  	vectorq1.x = x;
  	vectorq1.y = y;
  	vectorq1.z = z;
  	vectorq2.x = q.x;
  	vectorq2.y = q.y;
  	vectorq2.z = q.z;

  	VECTOR tempvec1;
  	VECTOR tempvec2;
  	VECTOR tempvec3;
  	tempvec1 = vectorq1;
  	q3.w = (w*q.w) - tempvec1.DotProduct(vectorq2);
  	tempvec1.CrossVector(vectorq2);
  	tempvec2.x = w * q.x;
  	tempvec2.y = w * q.y;
  	tempvec2.z = w * q.z;
  	tempvec3.x = q.w * x;
  	tempvec3.y = q.w * y;
  	tempvec3.z = q.w * z;
  	q3.x = tempvec1.x + tempvec2.x + tempvec3.x;
  	q3.y = tempvec1.y + tempvec2.y + tempvec3.y;
  	q3.z = tempvec1.z + tempvec2.z + tempvec3.z;
  	CopyQuat(q3);
}
